#include <app/app_imu.h>
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-10-08     huger       the first version
 */
#include "app_imu.h"
#include "bsp/bsp_can1.h"
#include "motor/motor.h"

#define DBG_TAG "imu"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>


void imu_init(void)
{
    bsp_can1_init();
}

bool imu_angel_get(float_t* rad)
{
    struct rt_can_msg msg;
    int8_t retry = 1;
    int16_t angle[3]={0};
    while(retry != 0)
    {
        msg.ide = RT_CAN_EXTID;        // 拓展格式
        msg.rtr = RT_CAN_DTR;          // 数据帧
        msg.len = 8;                   // 数据长度
        msg.id = 0x00A0;

        memset(msg.data, 0, sizeof(msg.data));
        msg.data[0] = 0x32;

        bsp_can1_tx_transmit(&msg);

        while(1)
        {
            if(bsp_can1_rx_transmit(&msg, 50) == false)
                break;

            if(msg.id == 0x306 && msg.len == 8)
            {
                angle[0] = ((msg.data[1] << 8) | msg.data[0] );
                angle[1] = ((msg.data[3] << 8) | msg.data[2] );
                angle[2] = ((msg.data[5] << 8) | msg.data[4] );

                rad[0]= angle[0] * 0.0001;//* RAD ;
                rad[1]= angle[1] * 0.0001;//* RAD ;
                rad[2]= angle[2] * 0.0001;//* RAD ;

                return true;
            }
        }
        retry--;
    }
    return false;
}

